// Copyright 2012 Sergio, Raul, Dorival. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#ifndef GEMLAB_ENRICH_H
#define GEMLAB_ENRICH_H

#include "mesh/algos.h"

namespace GemLab {

// enrich mesh with line and interface elements

void EnrichMeshBisect(Mesh * m, Array<Vec_t> const & Pts, Array<int> const & PtsTags, int LineTag, int JointTag, bool Quadratic = true, double Tol=1.0e-5) {

    // Points
    if (Pts.Size() != 2) {
        throw new Fatal("enrich.h: EnrichMeshBisect: number of points must be 2 for the time being");
    }
    for (size_t i=0; i<Pts.Size(); ++i) {
        if (Pts[i].rows() < 2 || Pts[i].rows() > 3) {
            throw new Fatal("enrich.h: EnrichMeshBisect: ndim of points must be 2 or 3");
        }
    }
    if (PtsTags.Size() != 2) {
        throw new Fatal("enrich.h: EnrichMeshBisect: number of PtsTags must be equal to 2. ntags=%zd is invalid", PtsTags.Size());
    }
    Vec_t X0 = Pts[0];
    Vec_t X1 = Pts[1];
    int ndim = X0.rows();

    //std::cout << ">> here1\n";

    // Initial conditions
    Vec_t  diff    = X1 - X0;
    double length  = diff.norm();
    double tinylen = Tol*length;
    double bdist   = 0.0;         // boundary function value
    double step    = length;      // length
    bool   first_segment = true;  // flag for the first segment

    //std::cout << ">> here2\n";

    // Defining required vectors
    Vec_t R(ndim);             // local coordinates
    Vec_t X  = X0;             // test point coordinates
    Vec_t Xp = X0;             // previous intersection point coordinates
    Vec_t T = (X1-X0)/length;  // unitary vector for the inset

    //std::cout << ">> here3\n";

    // Definig cell variables
    Cell const * init_cell = NULL;   // initial cell
    Cell const * curr_cell = NULL;   // cell at current point X
    Cell const * next_cell = NULL;   // next cell to the intersection

    //std::cout << ">> here4\n";

    // Find the initial and final cell
    init_cell  = FindCell(m, X0 + tinylen*T);

    if (init_cell==NULL) throw new Fatal("GemLab::EnrichMeshBisect: __internal_error__ FindCell failed");

    // Initializing variables
    curr_cell = init_cell;

    Vertex * P0 = NULL;
    Vertex * P1 = NULL;
    Vertex * P2 = NULL;
    bool Discrete = false;

    //std::cout << ">> here5\n";

    // Splitting inset
    do
    {
        diff = X1 - X;
        step = 0.5 * diff.norm();
        X += step*T;
        int n = static_cast<int>(log(step/Tol)/log(2.0) + 1);
        double step0 = 0.0;
        double dstep;

        //std::cout << ">> here6\n";

        for (int i=0; i<n; ++i)
        {
                
            CellInvMap(curr_cell, X, R);
            bdist = CellBryDist(curr_cell, R);
            step *= 0.5+Tol;

            // Bisection algorithm
            if (bdist>=-Tol) // (-Tol) is needed to aproximate the 'intersection' outside the cell
                X += step*T; // forward
            else
                X -= step*T; // backward

            dstep = std::abs(step - step0);
            step0 = step;
        }

        //std::cout << ">> here7\n";

        if (std::abs(dstep) > Tol) throw new Fatal("GemLab::EnrichMeshBisect: __internal_error__ Bisection failed");
        
        // Getting line points
        if (first_segment)
        {
            P0 = new Vertex;
            P0->Tag = PtsTags[0];
            for (int k=0; k<ndim; ++k) P0->C(k) = Xp(k);
        }
        else
            P0 = P1;

        //std::cout << ">> here8\n";

        // Second point
        P1 = new Vertex;
        P1->Tag = 0;
        for (int k=0; k<ndim; ++k) P1->C(k) = X(k);

        //std::cout << ">> here9\n";

        // Third point
        if (Quadratic)
        {
            P2 = new Vertex;
            P2->Tag = 0;
            for (int k=0; k<ndim; ++k) P2->C(k) = (Xp(k) + X(k)) / 2.0;
        }
        else
            P2 = NULL;

        //std::cout << ">> here10\n";

        // List of points
        Array<Vertex*> Ps(P0, P1);
        if (Quadratic)
            Ps.Push(P2);

        // Increasing vertices to the mesh
        for (int i=0; i<Ps.Size(); ++i) {
            if (i>0 || first_segment)
            {
                Ps[i]->ID = m->Verts.Size();
                m->Verts.Push(Ps[i]);
            }
        }

        // Saving line cell and joint(s) elements
        Cell * LineCell = new Cell;
        LineCell->ID    = m->Cells.Size();
        LineCell->Tag   = LineTag;
        LineCell->PartID = 0;
        if (!Quadratic) LineCell->Geo = LIN2; // line
        else            LineCell->Geo = LIN3; // quadratic_edge
        m->Cells.Push(LineCell);

        for (int i=0; i<Ps.Size(); ++i)
            LineCell->V.Push(Ps[i]);

        // Saving joint cell
        if (Discrete)
        {
            for (int i=0; i<Ps.Size(); ++i)
            {
                Cell * JointCell = new Cell;
                JointCell->Geo = JOINT;
                JointCell->Tag = JointTag;
                JointCell->PartID = 0;
                JointCell->V.Extend(curr_cell->V);
                JointCell->V.Push(Ps[i]);
                JointCell->JsldID = curr_cell->ID;
                JointCell->JlinID = LineCell->ID;
                JointCell->ID     = m->Cells.Size();
                m->Cells.Push(JointCell);
            }
        }
        else
        {
            Cell * JointCell = new Cell;
            JointCell->ID  = m->Cells.Size();
            JointCell->Geo = JOINT;
            JointCell->Tag = JointTag;
            JointCell->PartID = 0;
            JointCell->V.Extend(curr_cell->V);
            JointCell->V.Extend(LineCell->V);
            JointCell->JsldID = curr_cell->ID;
            JointCell->JlinID = LineCell->ID;
            m->Cells.Push(JointCell);
        }
        
        diff = X - X0;
        double curr_len = diff.norm();
        if (std::abs(curr_len-length) < Tol || curr_len>=length)
        {
            P1->Tag = PtsTags[1];
            return;
        }
                            
        // Find the next cell
        next_cell = FindCell(m, X + tinylen*T);
        if (next_cell==NULL) throw new Fatal("GemLab::EnrichMeshBisect: __internal_error__ FindCell failed");

        // Preparing for the next segment
        Xp = X;
        first_segment = false;
        curr_cell = next_cell;
    } while (true); 
}

}; // namespace GemLab

#endif // GEMLAB_ENRICH_H
